#include "pixel.h"
#include "common.h"
#include <stdio.h>

/**
 * @brief Interleave UV plane to one plane.
 * 
 * @param dst 
 * @param src_u 
 * @param src_v 
 * @param width 
 * @param height 
 */
void plane_copy_interleave(uint8_t *dst, uint8_t *src_u, uint8_t *src_v, int width, int height)
{
    int y = 0;
    for ( y = 0; y < height; y++)
    {
        for (int x = 0; x < width; x++)
        {
            dst[2*x]        = src_u[x];
            dst[2*x + 1]    = src_v[x];
        }

        src_u += width;
        src_v += width;
        dst += 2 * width;
    }
    
}

void plane_copy_deinterleave( uint8_t *dst_a, int i_dsta, uint8_t *dst_b, int i_dstb,
                                    uint8_t *src,  int i_src, int width, int height )
{
    for( int y=0; y < height; y++)
    {

        for( int x = 0; x<width; x++ )
        {
            dst_a[x] = src[2 * x];
            dst_b[x] = src[2 * x + 1];
        }
        dst_a += i_dsta;
        dst_b += i_dstb;
        src += i_src;
    }
}

#define HADAMARD4(d0, d1, d2, d3, s0, s1, s2, s3) {\
    uint32_t t0 = s0 + s1;\
    uint32_t t1 = s0 - s1;\
    uint32_t t2 = s2 + s3;\
    uint32_t t3 = s2 - s3;\
    d0 = t0 + t2;\
    d2 = t0 - t2;\
    d1 = t1 + t3;\
    d3 = t1 - t3;\
}
#define BITS_PER_SUM 16

static  uint32_t abs2( uint32_t a )
{
    uint32_t s = ((a>>(BITS_PER_SUM-1))&(((uint32_t)1<<BITS_PER_SUM)+1))*((uint16_t)-1);
    return (a+s)^s;
}

static int x264_pixel_satd_8x4(pixel *pix1, int i_pix1, pixel *pix2, int i_pix2)
{
    uint32_t tmp[4][4] = {0};
    uint32_t a0 = 0;
    uint32_t a1 = 0;
    uint32_t a2 = 0;
    uint32_t a3 = 0;
    uint32_t sum = 0;
    for( int i = 0; i < 4; i++, pix1 += i_pix1, pix2 += i_pix2 )
    {
        a0 = (uint32_t)(pix1[0] - pix2[0]) + ((uint32_t)(pix1[4] - pix2[4]) << BITS_PER_SUM);
        a1 = (uint32_t)(pix1[1] - pix2[1]) + ((uint32_t)(pix1[5] - pix2[5]) << BITS_PER_SUM);
        a2 = (uint32_t)(pix1[2] - pix2[2]) + ((uint32_t)(pix1[6] - pix2[6]) << BITS_PER_SUM);
        a3 = (uint32_t)(pix1[3] - pix2[3]) + ((uint32_t)(pix1[7] - pix2[7]) << BITS_PER_SUM);
        HADAMARD4( tmp[i][0], tmp[i][1], tmp[i][2], tmp[i][3], a0,a1,a2,a3 );
    }
    for( int i = 0; i < 4; i++ )
    {
        HADAMARD4( a0, a1, a2, a3, tmp[0][i], tmp[1][i], tmp[2][i], tmp[3][i] );
        sum += abs2(a0) + abs2(a1) + abs2(a2) + abs2(a3);
    }
    return (((uint16_t)sum) + (sum>>BITS_PER_SUM)) >> 1;
}

int x264_pixel_satd_8x8(pixel *pix1, int i_pix1, pixel *pix2, int i_pix2)
{
    int sum = x264_pixel_satd_8x4(pix1, i_pix1, pix2, i_pix2) + 
        x264_pixel_satd_8x4(pix1 + 4 * i_pix1, i_pix1, pix2 + 4 * i_pix2, i_pix2);
    return sum;
}

int x264_pixel_satd_16x16(pixel *pix1, int i_pix1, pixel *pix2, int i_pix2)
{
    int sum = x264_pixel_satd_8x4(pix1, i_pix1, pix2, i_pix2) + x264_pixel_satd_8x4(pix1 + 4 * i_pix1, i_pix1, pix2 + 4 * i_pix2, i_pix2);
    sum += x264_pixel_satd_8x4(pix1 + 8, i_pix1, pix2 + 8, i_pix2) + x264_pixel_satd_8x4(pix1 + 8 + 4 * i_pix1, i_pix1, pix2 + 8 + 4 * i_pix2, i_pix2);
    sum += x264_pixel_satd_8x4(pix1 + 8 * i_pix1, i_pix1, pix2 + 8 * i_pix2, i_pix2) + x264_pixel_satd_8x4(pix1 + 12 * i_pix1, i_pix1, pix2 + 12 * i_pix2, i_pix2);
    sum += x264_pixel_satd_8x4(pix1 + 8 + 8 * i_pix1, i_pix1, pix2 + 8 + 8 * i_pix2, i_pix2) + x264_pixel_satd_8x4(pix1 + 8 + 12 * i_pix1, i_pix1, pix2 + 8 + 12 * i_pix2, i_pix2);

    return sum;
}